#include "Arduino.h"
#include "arduinoPins.h"
#include "board.h"
#include "update_board.h"
#include "stdint.h"

#define MOTOR_A true
#define MOTOR_B false
#define CENTIMETER_A 1900
#define CENTIMETER_B 3750
#define SQUARE_A 5700
#define SQUARE_B 11250

int x_coord = 1;
int y_coord = 1;

void update_board(Move *move) {
  update_board(move->src, move->dest);
}

//If an input is 3+ volts it's HIGH, if it's 2- volts it's LOW
//for an output from arduino, 5 volts is HIGH, 0 volts is LOW
void update_board(Square src, Square dest) {
  digitalWrite(MAGNET, LOW);
  
  int32_t spaces_a = dest.row - src.row;
  int32_t spaces_b = dest.col - src.col;
  int32_t dir_a = (spaces_a >= 0) ? -1 : 1;
  int32_t dir_b = (spaces_b >= 0) ? -1 : 1;
  float ramp = .5;
  int32_t init_spaces_a = src.row - x_coord;
  int32_t init_spaces_b = src.col - y_coord;

  /*
	* calc number of steps from initial area to src square
   	*/
  int32_t distance_init_src_a = (-init_spaces_a) * SQUARE_A;
  int32_t distance_init_src_b = (-init_spaces_b) * SQUARE_B;
  /*
	* moves from initial position to source position
   	*/
  rotate( distance_init_src_a, .6, MOTOR_A );
  rotate( distance_init_src_b, .6, MOTOR_B );

  /*
   * number of steps from src to dest
   */
  int32_t distance_src_dest_a = abs(spaces_a) * SQUARE_A;
  int32_t distance_src_dest_b = abs(spaces_b) * SQUARE_B;
  
  digitalWrite(MAGNET, HIGH);
  delay(500);
  if(abs(spaces_a) == abs(spaces_b)) { //diagonal move
    Serial.println("starting diagonal");
    if(dir_a >= 0) digitalWrite(DIR_PIN_A, HIGH);
    else digitalWrite(DIR_PIN_A, LOW);
    if(dir_b >= 0) digitalWrite(DIR_PIN_B, HIGH);
    else digitalWrite(DIR_PIN_B, LOW);
    while(distance_src_dest_a >= 1 && distance_src_dest_b >= 2) {
        digitalWrite(STEP_PIN_A, HIGH);
        digitalWrite(STEP_PIN_B, HIGH);
        delayMicroseconds(100);
        digitalWrite(STEP_PIN_B, LOW);
        delayMicroseconds(100);
        digitalWrite(STEP_PIN_B, HIGH);
        delayMicroseconds(100);
        digitalWrite(STEP_PIN_A, LOW);
        digitalWrite(STEP_PIN_B, LOW);
        delayMicroseconds(100);
        distance_src_dest_a -= 1;
        distance_src_dest_b -= 2;
    }
    Serial.println("done with diagonal part");
    delay(500);
    ramp = 0.5;
    while(distance_src_dest_a != 0) {
      rotate((dir_a), ramp, MOTOR_A);
      distance_src_dest_a -= 1;
      ramp += 0.2;
    }
    ramp = .5;
    while(distance_src_dest_b != 0) {
      rotate((dir_b), ramp, MOTOR_B);
      distance_src_dest_b -= 1;
      ramp += 0.2;
    }
    digitalWrite(MAGNET, LOW);
    x_coord = dest.row;
    y_coord = dest.col;
    return;
  }
  
  /*
   * performs piece movement
   */
  while(distance_src_dest_a != 0 && digitalRead(BUTTON_A) == HIGH) {
    rotate((950 * dir_a), ramp, MOTOR_A);
    distance_src_dest_a -= 950;
    ramp += 0.2;
  }
  ramp = .5;
  while(distance_src_dest_b != 0 && digitalRead(BUTTON_B) == HIGH) {
    rotate((1875 * dir_b), ramp, MOTOR_B);
    distance_src_dest_b -= 1875;
    ramp += 0.2;
  }
  delay(500);
  digitalWrite(MAGNET, LOW);
  delay(500);
  digitalWrite(MAGNET, HIGH);
  delay(500);
  digitalWrite(MAGNET, LOW);
  
  x_coord = dest.row;
  y_coord = dest.col;
}

void rotate(int32_t steps, float speed, bool mtr) {
  //rotate a specific number of microsteps (8 microsteps per step) - (negative for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger
  int32_t dir = (steps > 0)? HIGH:LOW;
  int32_t dir_pin = (mtr) ? DIR_PIN_A : DIR_PIN_B;
  int32_t step_pin = (mtr) ? STEP_PIN_A : STEP_PIN_B;
  steps = abs(steps);

  digitalWrite(dir_pin, dir);

  if(speed > 1) {
    speed = 1;
  }

  float usDelay = (1/speed) * 70;

  for(int32_t i=0; i < steps; i++){
    digitalWrite(step_pin, HIGH);
    delayMicroseconds(usDelay); 

    digitalWrite(step_pin, LOW);
    delayMicroseconds(usDelay);
  }
}

